#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/LaserScan.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>


#include <cmath>

class PointCloudToLaserScan
{
public:
  PointCloudToLaserScan()
      : tf_listener_(tf_buffer_)
  {
    ros::NodeHandle private_nh("~");

    // 参数读取
    private_nh.param("pointcloud_topic_in", pointcloud_topic_in_, std::string("/cloud_registered"));
    private_nh.param("laserscan_topic_out", laserscan_topic_out_, std::string("/scan"));
    private_nh.param("min_height_offset", min_height_offset_, 0.1);
    private_nh.param("max_height_offset", max_height_offset_, 1.0);
    private_nh.param("angle_min", angle_min_, -M_PI);
    private_nh.param("angle_max", angle_max_, M_PI);
    private_nh.param("angle_increment", angle_increment_, 0.005);
    private_nh.param("scan_time", scan_time_, 0.1);
    private_nh.param("range_min", range_min_, 0.1);
    private_nh.param("range_max", range_max_, 30.0);
    private_nh.param("use_inf", use_inf_, true);
    private_nh.param("inf_epsilon", inf_epsilon_, 1.0);
    private_nh.param("if_debug", if_debug_, false);

    // 订阅与发布
    cloud_sub_ = nh_.subscribe(pointcloud_topic_in_, 10, &PointCloudToLaserScan::pointCloudCallback, this);
    scan_pub_ = nh_.advertise<sensor_msgs::LaserScan>(laserscan_topic_out_, 10);

    if (if_debug_)
    {
      debug_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/point_filted", 10);
      ROS_INFO("Debug mode enabled: publishing filtered clouds on /point_filted");
    }

    ROS_INFO("PointCloud to LaserScan converter initialized");
  }

private:
  void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr &cloud_msg)
  {
    // 1. 直接将点云转换到body坐标系 - 使用tf2_sensor_msgs进行转换
    sensor_msgs::PointCloud2 body_cloud2;

    try
    {
      // 创建转换请求
      geometry_msgs::TransformStamped transform =
          tf_buffer_.lookupTransform("body", cloud_msg->header.frame_id,
                                     cloud_msg->header.stamp, ros::Duration(0.1));

      // 使用tf2进行点云转换
      tf2::doTransform(*cloud_msg, body_cloud2, transform);
    }
    catch (const tf2::TransformException &ex)
    {
      ROS_WARN("TF transform failed: %s", ex.what());
      return;
    }

    // 2. 将转换后的PointCloud2转换为PCL点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr body_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(body_cloud2, *body_cloud);

    // 3. 在body坐标系下进行高度滤波（使用相对于机体的高度偏移）
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(body_cloud);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(min_height_offset_, max_height_offset_);
    pass.filter(*filtered_cloud);

    // 调试模式下发布滤波后点云
    if (if_debug_)
    {
      sensor_msgs::PointCloud2 debug_msg;
      pcl::toROSMsg(*filtered_cloud, debug_msg);
      debug_msg.header.frame_id = "body_2d";
      debug_msg.header.stamp = cloud_msg->header.stamp;
      debug_pub_.publish(debug_msg);
    }

    // 4. 生成 LaserScan
    sensor_msgs::LaserScan scan_msg;
    scan_msg.header.stamp = cloud_msg->header.stamp;
    scan_msg.header.frame_id = "body_2d";
    scan_msg.angle_min = angle_min_;
    scan_msg.angle_max = angle_max_;
    scan_msg.angle_increment = angle_increment_;
    scan_msg.time_increment = 0.0;
    scan_msg.scan_time = scan_time_;
    scan_msg.range_min = range_min_;
    scan_msg.range_max = range_max_;

    // 计算扫描点数并初始化范围数组
    size_t num_readings = static_cast<size_t>(
        std::ceil((scan_msg.angle_max - scan_msg.angle_min) / scan_msg.angle_increment));
    scan_msg.ranges.assign(num_readings,
                           use_inf_ ? std::numeric_limits<float>::infinity()
                                    : range_max_ + inf_epsilon_);

    // 处理每个点
    for (const auto &pt : filtered_cloud->points)
    {
      // 计算点云在XY平面的角度和距离
      double angle = std::atan2(pt.y, pt.x);
      double range = std::hypot(pt.x, pt.y);

      // 跳过超出角度范围的点
      if (angle < angle_min_ || angle > angle_max_)
        continue;

      // 跳过超出距离范围的点
      if (range < range_min_ || range > range_max_)
        continue;

      // 计算扫描索引（使用四舍五入避免浮点误差）
      int idx = static_cast<int>(std::round((angle - angle_min_) / angle_increment_));

      // 确保索引在有效范围内
      if (idx >= 0 && idx < static_cast<int>(num_readings))
      {
        // 保存最小距离值
        if (range < scan_msg.ranges[idx])
        {
          scan_msg.ranges[idx] = static_cast<float>(range);
        }
      }
    }

    scan_pub_.publish(scan_msg);
  }

  ros::NodeHandle nh_;
  ros::Subscriber cloud_sub_;
  ros::Publisher scan_pub_;
  ros::Publisher debug_pub_;
  tf2_ros::Buffer tf_buffer_;
  tf2_ros::TransformListener tf_listener_;

  std::string pointcloud_topic_in_;
  std::string laserscan_topic_out_;
  bool if_debug_;

  double min_height_offset_;
  double max_height_offset_;
  double angle_min_;
  double angle_max_;
  double angle_increment_;
  double scan_time_;
  double range_min_;
  double range_max_;
  bool use_inf_;
  double inf_epsilon_;
};

int main(int argc, char **argv)
{
  ros::init(argc, argv, "dynamic_height_pointcloud_to_laserscan");
  PointCloudToLaserScan node;
  ros::spin();
  return 0;
}